#include "ltc2496.h"
#ifdef USE_FREERTOS
#include "STM32FreeRTOS.h"
#endif
void ltc2496::begin(YSPI *_spi, const u8 *_chan, u8 _nrs, u16 _zero) {
    this->spi = _spi;
    this->chan = _chan;
    this->nrs = _nrs;
    this->zero = _zero;
    value = (u16 *) malloc(nrs * 2);
#ifdef USE_DEB
    fil = (debouncer **)malloc(nrs * 4);
    for (int i = 0; i < nrs; i++)
        fil[i] = new debouncer(65535, 328, 5, 2);
#endif
#ifdef USE_FREERTOS
    osThreadDef(ltc, this->thread, osPriorityRealtime, 0, 96);
    osThreadCreate(osThread(ltc), this);
#endif
    // u8 cmd = 0xb0 | 0;
    // spi->beginTransaction();
    // spi->transfer(cmd);
    // spi->transfer(cmd);
    // spi->transfer(cmd);
    // spi->endTransaction();
}

u16 ltc2496::read(u8 channel)
{
    float v = (int)value[channel] - (int)zero;
    v = v * 0xffff / (0xffff - zero);
    return v > 50 ? v : 0;
}

#ifdef USE_FREERTOS
void ltc2496::thread(const void *_this)
{
    ((ltc2496 *)_this)->rtos_loop();
}

void ltc2496::rtos_loop()
{
    while (1)
    {
        union dat
        {
            u8 r[4];
            u32 v;
        } u;
        u.r[3] = 0;
        u8 cmd = 0xb0 | chan[index];
        spi->beginTransaction();
        u32 tick = millis() + 200;
        while (spi->miso() && millis() < tick)
        {
            vTaskDelay(1);
        }
        u.r[2] = spi->transfer(cmd);
        u.r[1] = spi->transfer(0);
        u.r[0] = spi->transfer(0);
        spi->endTransaction();
        u16 val;
        if ((u.r[2] & 0xc0) == 0)
        {
            if (u.r[2] & 0x10) {
                val = ((u.v >> 4) & 0x1ffff) - 1;
            } else {
                val = ((u.v >> 4) & 0x1ffff);
            }
            if ((u.r[2] & 0x20) == 0) {
                val = -val;
            }
#ifdef USE_DEB
            value[last] = fil[last]->process(val);
#else
            // if (val > lastValue[last] && (val - lastValue[last]) > 65535 * 0.3)
            //     val = lastValue[last] + 65535 * 0.3;
            // else if (val < lastValue[last] && (lastValue[last] - val) > 65535 * 0.3)
            //     val = lastValue[last] - 65535 * 0.3;
            value[last] = lastValue[last] * 0.8 + (float) val * 0.2;
            lastValue[last] = val;
#endif
            // if (value[last] > 65500 || value[last] < 800)
            //     value[last] = 0;
            last = index;
            index++;
            if (index >= nrs)
                index = 0;
        }
        vTaskDelay(30);
    }
}
#endif